#include "Steering.h"

Vector3D Steering::Normalize(Vector3D v3)
{
	v3.CalculateMagnitude();
	Vector3D normalized(v3.getX() / v3.getMagnitude(), v3.getY() / v3.getMagnitude(), v3.getZ() / v3.getMagnitude());

	return normalized;
}

//this should take it a pointer to whatever velocity it should be modifying
//for example, if Seek() uses Sleep(), then Sleep will modify that velocity to be 0
//not just the velocity in general like it is now
void Steering::Rest()
{
	pOwnedObject->setVelocity(0, 0, 0);
}

//Seek sets the velocity vector of MovingObject it's attached to
void Steering::Seek(MovingObject* target)
{
	//checks to see if the object has reached it's target
	if(pOwnedObject->isAtTarget(target->getPosition()))
	{
		//if it has, stop moving
		Rest();
	}
	else
	{
		//sets the desired angle it should turn in order to face the correct way
		//for seek, this is looking directly at it's target
		float angle = atan2(target->getPosition().getY() - pOwnedObject->getPosition().getY(),
			(-1 * target->getPosition().getX()) - (-1 * pOwnedObject->getPosition().getX()));



		//determines correct velocity to get to target
		Vector3D DesiredVelocity = Normalize(target->getPosition() - pOwnedObject->getPosition()) * pOwnedObject->getSpeed();

		pOwnedObject->setVelocity(DesiredVelocity);
		pOwnedObject->setDesiredAngle(angle);
	}
}
void Steering::Flee(MovingObject* target)
{

	float angle = atan2(target->getPosition().getY() - pOwnedObject->getPosition().getY(),
							(-1 * target->getPosition().getX()) - (-1 * pOwnedObject->getPosition().getX()));
	angle+= PI;
	
	if(angle < 0)
		angle += (2 * PI);
	else if(angle > 2 * PI)
		angle -= (2 * PI);

	Vector3D DesiredVelocity = Normalize(pOwnedObject->getPosition() - target->getPosition()) * pOwnedObject->getSpeed();

	pOwnedObject->setVelocity(DesiredVelocity);
	pOwnedObject->setDesiredAngle(angle);


}

void Steering::Evade(MovingObject* target)
{
	//COPYPASTA TASTES LIKE RAMEN - Big Brandon 
	MovingObject futureTarget = *target;
	if(fabs(target->getPosition().DistanceTo(pOwnedObject->getPosition()))<40)
	{
		for(int i = 0;i<9;++i)
		{
			futureTarget.setVelocity(futureTarget.getVelocity() + futureTarget.getAccel() * .01);
			futureTarget.setPosition(futureTarget.getPosition() + futureTarget.getVelocity() *.01);
		}
			Flee(target);
	}
	else Rest();


}
///////////////////////////////////////////////////////////////////////////////////////////////
void Steering::Pursuit(MovingObject* target)
{

	//creates a new MovingObject representing where the target will be, instead of where it currently is
	MovingObject futureTarget = *target;

	//figures out what the target's position will be 30 steps from now
	//instead of looping through the update functions a whole bunch of times, let's just use the position formula
	//futureTarget->setPosition(p(0) + v(0)*t + .5at^2)
	//futureTarget.setPosition(futureTarget.getPosition() + futureTarget.getVelocity() * 30 + (futureTarget.getAccel() * .5) * 30 * 30);
	for(int i = 0;i<9;++i)
	{
		futureTarget.setVelocity(futureTarget.getVelocity() + futureTarget.getAccel() * .01);
		futureTarget.setPosition(futureTarget.getPosition() + futureTarget.getVelocity() * .01);
		//futureTarget->Update(dt);
	}

	Seek(&futureTarget);

	/*pOwnedObject->setTarget(v3FuturePosition);
	Vector3D DesiredVelocity = Normalize(pOwnedObject->getTarget() - pOwnedObject->getPosition()) * pOwnedObject->getSpeed();
	pOwnedObject->setVelocity(DesiredVelocity);*/
}
//////////////////////////////////////////////////////////////////////////////////////////////////